#ifndef FEATURE_H
#define FEATURE_H

#include <memory>
#include <map>
#include <Eigen/Dense>
#include "../util/mathfunc.h"

#include "Frame.h"

namespace Estimator
{

class Frame;

class LandMarkPLX //FeaturePLX
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    enum STATUS
    {
      READY = 0,
      NOT_READY = 1, // will be set not ready when it is initialized
      NEED_MORE_FRAME = 2
    };

    LandMarkPLX() = delete;
    virtual ~LandMarkPLX() = default;

    // Construct a LandMarkPLX variable at first glance.
    LandMarkPLX(size_t id, 
                const Frame *paramMainAnchor, const Eigen::Vector3d &paramMainUV) : 
                mId_(id), mStatus_(NOT_READY), mpMainAnchor_(paramMainAnchor), mDirectionMain_(paramMainUV)
    {}

    void SetAssociateFrame(const Frame *pAssoAnchor, const double cosParallax, STATUS status)
    {
      mpAssoAnchor_ = pAssoAnchor;
      mCosParallaxAngle_ = cosParallax; 
      mSinParallaxAngle_ = sqrt(1 - cosParallax * cosParallax);
      mStatus_ = status;
    }

    void SetParallaxAngle(double cosx, double sinx) // only set the parallax angle
    {
      mCosParallaxAngle_ = cosx;
      mSinParallaxAngle_ = sinx;
      // mSinParallaxAngle_ = sqrt(1 - cosx * cosx);
    }
    
    const size_t GetId() const { return mId_; }
    const Frame *GetMainFrame() const { return mpMainAnchor_; }
    const Frame *GetAssoFrame() const { return mpAssoAnchor_; }
    const Eigen::Vector3d& GetDirectionMain() const { return mDirectionMain_; }
    STATUS GetStatus() const {return mStatus_;}
    double GetCosParallax() const {return mCosParallaxAngle_; }
    double GetSinParallax() const {return mSinParallaxAngle_; }

    void AddObservation(const Frame* pKF, size_t idx)
    {
      if(!mMapObservations_.count(pKF))
        mMapObservations_[pKF] = idx;
    }

    // 
    void SetInitializationParameter(const Calibration& calibration)
    {
      // terrible code for setting..  
      if(!mpMainAnchor_ || !mpAssoAnchor_){
        std::cerr << "can't set the initialization parameter for the landmark. ID: " << mId_ << std::endl;
        return;
      }
      
      // First, get the cos(angle), angle is the vector between the p_m - p_a and direction vector of the main anchor.
      Eigen::Vector3d DirMainAnchor = mpMainAnchor_->mRotationMatrix * calibration.Rbc * mDirectionMain_;
      DirMainAnchor.normalize();
      Eigen::Vector3d pmpa = mpMainAnchor_->mTranslationVec -  mpAssoAnchor_->mTranslationVec ;
      pmpa.normalize();
      mCosAlpha_ = pmpa.dot(DirMainAnchor);
      mSinAlpha_ = sqrt(1-mCosAlpha_*mCosAlpha_); // should we consider the -sin(angle)?
      double sinAngleMtheta = mSinAlpha_ * mCosParallaxAngle_ - mCosAlpha_ * mSinParallaxAngle_; // sin(alpha - theta)
      Eigen::Vector3d nzj = DirMainAnchor.cross(pmpa); // direction X (pm - pa)
      nzj.normalize();
      double angle = acos(mCosAlpha_);
      Eigen::Vector3d temp = nzj*(3.1415926 - angle);
      mBzj_.setZero();
      mBzj_ = sinAngleMtheta * Exp(temp);
      // std::cout << "Bzj: " << mBzj_ << std::endl;

    }

    Eigen::Matrix3d GetInitializationParameter() const {return mBzj_; }

    std::map<const Frame*, size_t> GetObservations() const { return mMapObservations_;}

    friend std::ostream& operator<<(std::ostream& os, const LandMarkPLX& landmarkPLX);

  private:
    size_t mId_;
    double mCosParallaxAngle_ = 100.0;  // can't be set to negative value!
    double mSinParallaxAngle_ = 100.0;
    STATUS mStatus_;

    // To do here: think about whether should be changed into shared_ptr
    // const Calibration* mpCalibration_ = nullptr;
    const Frame *mpMainAnchor_ ;
    const Frame *mpAssoAnchor_ = nullptr ;

    // this has the information of this landmark's observations in all frame that observes it.
    // it has the pointer for frame , and index of that frame's observation vector. 
    std::map< const Frame*, size_t> mMapObservations_;

    Eigen::Vector3d mDirectionMain_; // Direction in the main anchor's body frame,  need to be normalized
    // Eigen::Vector3d mDirectionAsso_; // Direction in the associate anchor's body frame,  need to be normalized

    // parameter for the initialzation.
    double mSinAlpha_; //sin value of the angle between the vector of associate frame and feature.
    double mCosAlpha_; //cos value of ..
    Eigen::Matrix3d mBzj_ ; // sin(ahpla - theta) * exp(nzj (pi - alpha)) 
};

inline std::ostream& operator << (std::ostream& os, const LandMarkPLX& landmarkplx)
{
    // os << timer.timer_name_ << " takes " << std::chrono::duration_cast<std::chrono::microseconds>(timer.end_ - timer.start_).count() << "us. ";
    if(landmarkplx.mpAssoAnchor_)
      os << landmarkplx.mId_ << " " << landmarkplx.GetMainFrame()->GetFrameId() << " " << landmarkplx.GetAssoFrame()->GetFrameId() << " "
         << landmarkplx.mCosParallaxAngle_ << " " << landmarkplx.mSinParallaxAngle_ ;
    else
      os << "NOT READY Yet";
    return os;
}


}

#endif // FEATURE_H